solver_ceres.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
// Ceres includes
#include "ceres/jet.h"
#include "ceres/ceres.h"
#include "glog/logging.h"
 
// wolf includes
#include "core/solver/solver_manager.h"
#include "yaml-cpp/yaml.h"
 
namespace ceres
{
typedef std::shared_ptr<CostFunction> CostFunctionPtr;
}
 
namespace wolf
{
WOLF_PTR_TYPEDEFS(SolverCeres);
WOLF_PTR_TYPEDEFS(ManifoldWrapper);
 
class SolverCeres : public SolverManager
{
  protected:
    std::map<FactorBasePtr, ceres::ResidualBlockId> fac_2_residual_idx_;
    std::map<FactorBasePtr, ceres::CostFunctionPtr> fac_2_costfunction_;
 
    // this map is only for handling automatic destruction of ManifoldWrapper objects
    std::map<StateBlockPtr, ManifoldWrapperPtr> state_blocks_manifold_;
 
    ceres::Solver::Summary             summary_;
    std::unique_ptr<ceres::Problem>    ceres_problem_;
    std::unique_ptr<ceres::Covariance> covariance_;
 
    // profiling
    unsigned int n_iter_acc_, n_iter_max_;
    unsigned int n_convergence_, n_interrupted_, n_no_convergence_;
 
  public:
    SolverCeres(const ProblemPtr& _wolf_problem, const YAML::Node& _params);
 
    WOLF_SOLVER_CREATE(SolverCeres);
 
    ~SolverCeres() override;
 
    ceres::Solver::Summary getSummary() const;
 
    std::unique_ptr<ceres::Problem>& getCeresProblem();
 
    bool computeCovariancesDerived(
        CovarianceBlocksToBeComputed _blocks = CovarianceBlocksToBeComputed::ROBOT_LANDMARKS) override;
 
    bool computeCovariancesDerived(const std::vector<StateBlockPtr>& st_list) override;
 
    bool         converged() const override;
    bool         failed() const override;
    bool         wasStopped() const override;
    unsigned int iterations() const override;
    double       initialCost() const override;
    double       finalCost() const override;
    double       totalTime() const override;
 
    ceres::Solver::Options& getSolverOptions();
 
    const Eigen::SparseMatrixd computeHessian() const;
 
    virtual int numStateBlocksEstimatedDerived() const override;
 
    virtual int numFactorsDerived() const override;
    virtual int sizeStateBlocksEstimatedDerived() const override;
    virtual int sizeFactorsDerived() const override;
 
  protected:
    bool checkDerived(std::string prefix = "") const override;
 
    std::string solveDerived(const ReportVerbosity report_level) override;
 
    void addFactorDerived(const FactorBasePtr& fac_ptr) override;
 
    void removeFactorDerived(const FactorBasePtr& fac_ptr) override;
 
    void addStateBlockDerived(const StateBlockPtr& state_ptr) override;
 
    void removeStateBlockDerived(const StateBlockPtr& state_ptr) override;
 
    void updateStateBlockStatusDerived(const StateBlockPtr& state_ptr) override;
 
    void updateStateBlockLocalParametrizationDerived(const StateBlockPtr& state_ptr) override;
 
    ceres::CostFunctionPtr createCostFunction(const FactorBasePtr& _fac_ptr);
 
    bool isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const override;
 
    bool isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const override;
 
    bool isStateBlockFixedDerived(const StateBlockPtr& st) const override;
 
    bool hasThisLocalParametrizationDerived(const StateBlockPtr&               st,
                                            const LocalParametrizationBasePtr& local_param) const override;
 
    bool hasLocalParametrizationDerived(const StateBlockPtr& st) const override;
 
    void printProfilingDerived(std::ostream& _stream) const override;
 
    // PARAMS
    ceres::Solver::Options solver_options_;
};
 
inline ceres::Solver::Summary SolverCeres::getSummary() const
{
    return summary_;
}
 
inline std::unique_ptr<ceres::Problem>& SolverCeres::getCeresProblem()
{
    return ceres_problem_;
}
 
inline ceres::Solver::Options& SolverCeres::getSolverOptions()
{
    return solver_options_;
}
 
inline bool SolverCeres::isFactorRegisteredDerived(const FactorBasePtr& fac_ptr) const
{
    return fac_2_residual_idx_.count(fac_ptr) == 1 and fac_2_costfunction_.count(fac_ptr) == 1;
}
 
inline bool SolverCeres::isStateBlockRegisteredDerived(const StateBlockPtr& state_ptr) const
{
    if (state_blocks_.count(state_ptr) == 0) return false;
    return ceres_problem_->HasParameterBlock(getAssociatedMemBlockPtr(state_ptr));
}
 
inline bool SolverCeres::isStateBlockFixedDerived(const StateBlockPtr& st) const
{
    if (state_blocks_.count(st) == 0) return false;
    return ceres_problem_->IsParameterBlockConstant(getAssociatedMemBlockPtr(st));
};
 
inline bool SolverCeres::hasLocalParametrizationDerived(const StateBlockPtr& st) const
{
    return state_blocks_manifold_.count(st) == 1;
};
 
inline int SolverCeres::numFactorsDerived() const
{
    return ceres_problem_->NumResidualBlocks();
};
 
inline int SolverCeres::sizeFactorsDerived() const
{
    return ceres_problem_->NumResiduals();
}
 
}  // namespace wolf